#include "main.h"

volatile M1States M1_State = M1_INIT;
volatile struct_M1_Flag M1_Flag = {0};

//volatile int16_t Motor_Iq_Set_Buf = 0;
volatile uint32_t u_MotorRmpSet = 30;
volatile uint32_t FlagLoopEn = 0;
static uint32_t M1_Block_Time = 0;
volatile uint32_t M1_Speed_Ctrl_Mode = SPEED_LOOP;
static uint32_t M1_Last_Speed_Ctrl_Mode = SPEED_LOOP;


void TMR0_Config(void)
{
	SYS_EnablePeripheralClk(SYS_CLK_TIMER01_MSK);
	TMR_ConfigClk(TIMER0,TMR_CLK_DIV_16);		/*4MHz*/

	TMR_ConfigRunMode(TIMER0,TMR_COUNT_PERIOD_MODE, TMR_BIT_32_MODE);
	TMR_DisableOneShotMode(TIMER0);
	
//	TMR_SetPeriod(TMR0,0XFFFFFFFF);			/*1000ms = 3000*Fclk*/
	Reset_T0_Cnt();
	
	TMR_DisableOverflowInt(TIMER0);
	TMR_ClearOverflowIntFlag(TIMER0);
	
	TMR_Start(TIMER0);
	
	//TMR_Stop(TIMER0);
}


void M1_Init(void)
{
	M1_Flag.RunStop = 0;
	M1_Flag.MotorFR = FR_CW;
	M1_Flag.MotorFR_Pre = M1_Flag.MotorFR;
	
	M1_Flag.CaliEn = 0;
	TMR0_Config();
	

}

void M1_Para_init(void)
{
	RPM_Ref = 0;
	Motor_Iq_Set = 0;
	RPM_Set = 0;
	Speed_Ref = 0;
	//u_MotorRmpSet = 0;
	Speed_Integer = 0;
	M1_Block_Time = 0;
	M1_Last_Speed_Ctrl_Mode = M1_Speed_Ctrl_Mode;
	Motor_Iq_Set_Max = 30000;
}

int32_t M1_Speed_Filter(uint32_t speed)
{
	#define FILTER_NUM 10
	#define FILTER_SHIFT_NUM 3

	static int32_t speedBuf[FILTER_NUM];
	static int32_t in = 0;
	static int32_t out = 0;
	static int32_t speedSum = 0;
	static int32_t calSpeed;
	static int32_t max,min;

	speedBuf[in] = speed;
	speedSum += speed;
	in++;
	if(in >= FILTER_NUM) in = 0;
	
	if(in == out)
	{
		max = 0;
		min = 0xffff;
		for(int i= 0;i<FILTER_NUM;i++)
		{
			if(speedBuf[i] > max) max = speedBuf[i];
			if(speedBuf[i] < min) min = speedBuf[i];
		}
		
		calSpeed = speedSum - max - min;
		calSpeed >>= FILTER_SHIFT_NUM;
		speedSum -= speedBuf[out];
		out++;
		if(out >= FILTER_NUM) out = 0;

		
	}
	else
	{
		calSpeed = 0;
	}

	return calSpeed;
}

void M1_Cal_Speed(void)
{
	static uint32_t sectorTime = 0;
	
	static uint32_t last_Angle = 0;

	static int32_t currAngle;
	static int32_t tolAngle;

	//log_i("%u",SC60228_Data);

	currAngle = SC60228_Data;
	sectorTime = Load_T0_Cnt();
	Reset_T0_Cnt();

	/*
	if(sectorTime > 3000000*4)
	{
		Basic.Calb_Speed = 0;
		Basic.Mech_Speed = 0;	
		last_Angle = currAngle;
	}
	*/
	
	tolAngle = ((currAngle + 4096 )- last_Angle)%4096;
		
	last_Angle = currAngle;
	
	Basic.Calb_Speed = (58594ul*tolAngle)/(sectorTime);
		
		
	Basic.Mech_Speed = M1_Speed_Filter(Basic.Calb_Speed);
		
	
}

#if 0
void M1_Cal_Speed(void)
{
	static int32_t Mech_Speed;

	Mech_Speed = PLL_Speed;
	Mech_Speed *= 60;
	Mech_Speed >>= 9;
	Mech_Speed /= Motor.PPN;

	Basic.Mech_Speed = Mech_Speed;

}
#endif

void M1_Speed_Ctrl(void)
{

	static uint32_t speedSetStep = 50;
	static uint32_t speedAdjustTime = 0;

	if((Motor.En == 1) && (Motor.Debug == 0))
	{
		
		if(++speedAdjustTime >= 10)
		{
			speedAdjustTime = 0;

			if(RPM_Set < u_MotorRmpSet)
			{
				RPM_Set += speedSetStep;
				if(RPM_Set > u_MotorRmpSet) RPM_Set = u_MotorRmpSet;
			}
			else if(RPM_Set > u_MotorRmpSet)
			{
				if(RPM_Set > speedSetStep)
				{
					RPM_Set -= speedSetStep;
					if(RPM_Set < u_MotorRmpSet) RPM_Set = u_MotorRmpSet;
				}
				else
				{

				}
			}
		}

	}
	else
	{
		
	}

}

uint32_t M1_Check_Block(void)
{
	M1_Block_Time++;
	if(Basic.Mech_Speed > 10)
	{
		M1_Block_Time = 0;
	}
	if(M1_Block_Time > 1500)
	{
		
		M1_Block_Time = 0;
		log_e("M1_Check_Block");
		return 1;
	}

	return 0;
}

void M1_Task_1ms(void)
{
	static M1States last_M1_STATE = M1_UNKNOWN;
	static uint32_t flagM1StateChanged;
	static uint32_t M1_State_RunTime = 0;
	
	

	if(last_M1_STATE != M1_State)
	{
		last_M1_STATE = M1_State;
		flagM1StateChanged = 1;
		M1_State_RunTime = 0;
		putM1state();
	}
	else
	{
		flagM1StateChanged = 0;
		M1_State_RunTime++;
	}

//	if(flagM1StateChanged)
//	{
//		log_d("M1_State = %u",M1_State);
//	}

	switch(M1_State)
	{
		case M1_INIT:
			if(M1_Flag.RunStop)
			{
				M1_Para_init();
				M1_State = M1_RUN;
			}
			break;
		case M1_STOP:
			if(M1_Flag.RunStop)
			{
				M1_State = M1_RUN;
			}
			else
			{
				M1_State = M1_INIT;
			}
			break;
		case M1_RUN:
			if(M1_Flag.RunStop)
			{
				Motor.En = 1;
			}
			else
			{
				Motor.En = 0;
				M1_State = M1_STOP;
			}

			if(M1_Flag.CaliEn == 0)
			{

			if((M1_Last_Speed_Ctrl_Mode != M1_Speed_Ctrl_Mode) || M1_Check_Block())
			{
				Motor.En = 0;
				M1_State = M1_BRAKE;
			}
			}
			
			break;
		case M1_BRAKE:
			
			if(M1_State_RunTime > 500)
			{
				M1_State = M1_INIT;
				
			}
			break;
		case M1_FAULT:

			break;
		
	}

	#if(OPEN_LOOP_EN)

	Motor.En = 1;

	#else

	M1_Speed_Ctrl();

	#endif

	M1_Cal_Speed();
}


